#!/usr/bin/env roseus

(ros::load-ros-manifest "jskpointcloud")
(load "package://jskpointcloud/euslisp/jskpointcloud.l")
(ros::load-ros-manifest "human_tracking")

(defun human-pub (msg &key (test nil))
  (let ((pos-lst (list)))
    (dolist (h (send msg :array))
      (let ((torso-cds
	     (ros::tf-pose->coords
	      (elt (send h :coords_list)
		   (position "torso" (send h :link_list) :test #'string=))))
	    cds pub-3dp pub-msg)
	(send *tf-listener* :wait-for-transform
	      *base-frame* *cam-frame* (ros::time 0) 10)
	(if test
	    (setq cds (make-coords))
	  (setq cds (send *tf-listener* :lookup-transform
			  *base-frame* *cam-frame*
			  (ros::time 0))))
	(print (list 'before-transform-torso (send torso-cds :worldpos)))
	(push
	 (send cds :transform-vector (send torso-cds :worldpos))
	 pos-lst)))
    (print (list 'torso pos-lst))
    (setq pub-3dp (instance 3dpointcloud :init :points
			    pos-lst))
    (setq pub-msg (make-msg-from-3dpointcloud
		   pub-3dp :frame *base-frame*))
    (ros::publish *pub-people-topic* pub-msg)))

(ros::roseus "body_subscriber")
;;(setq *pub-people-topic* "people_with_socks")
(setq *pub-people-topic* "/people_points")
(setq *tf-listener* (instance ros::transform-listener :init))
(setq *body-topic* "/openni_tracker/bodies")
(ros::subscribe *body-topic* human_tracking::BodyArray #'human-pub)
(setq *base-frame* "/base_footprint")
(setq *cam-frame* "/openni_rgb_optical_frame")
(ros::advertise *pub-people-topic* sensor_msgs::PointCloud 1)

(setq *indices* nil)
(setq *cds* (make-coords))

(defun ros-spin ()
  (do-until-key
   (ros::spin-once)))


(ros-spin)